class for Motion processors More...

#include <core/processor/processor_motion.h>

Inheritance diagram for wolf::ProcessorMotion:

Public Types
enum	ProcessingStep { FIRST_TIME_WITHOUT_KF , FIRST_TIME_WITH_KF_BEFORE_INCOMING , FIRST_TIME_WITH_KF_ON_INCOMING , FIRST_TIME_WITH_KF_AFTER_INCOMING , RUNNING_WITHOUT_KF , RUNNING_WITH_KF_BEFORE_ORIGIN , RUNNING_WITH_KF_ON_ORIGIN , RUNNING_WITH_KF_AFTER_ORIGIN }

Public Member Functions
	ProcessorMotion (const std::string &_type, TypeComposite _state_types, SizeEigen _state_size, SizeEigen _delta_size, SizeEigen _delta_cov_size, SizeEigen _data_size, SizeEigen _calib_size, const YAML::Node &_params)

	~ProcessorMotion () override

virtual void	resetDerived ()

TimeStamp	getTimeStamp () const override
	Get the timestamp of the last motion. More...

VectorComposite	getState (StateKeys _keys="") const override
	Get the state at the last time. More...

VectorComposite	getState (const TimeStamp &_ts, StateKeys _keys="") const override
	Get the state at a specific time. More...

const Eigen::MatrixXd	getCurrentDeltaPreintCov () const
	Gets the delta preintegrated covariance from all integrations so far. More...

Motion	getMotion () const
	Provide the motion integrated so far. More...

Motion	getMotion (const TimeStamp &_ts) const
	Provide the motion integrated until a given timestamp. More...

CaptureMotionConstPtr	findCaptureContainingTimeStamp (const TimeStamp &_ts) const
	Finds the capture that contains the closest previous motion of _ts. More...

CaptureMotionPtr	findCaptureContainingTimeStamp (const TimeStamp &_ts)

void	setOrigin (FrameBasePtr _origin_frame)

FrameBasePtr	setOrigin (const VectorComposite &_x_origin, const TimeStamp &_ts_origin)

MotionBuffer &	getBuffer ()

const MotionBuffer &	getBuffer () const

virtual void	computeCurrentDelta (const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const double _dt, Eigen::VectorXd &_delta, Eigen::MatrixXd &_delta_cov, Eigen::MatrixXd &_jacobian_calib) const =0
	convert raw CaptureMotion data to the delta-state format More...

virtual void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _dt2, Eigen::VectorXd &_delta1_plus_delta2) const =0
	composes a delta-state on top of another delta-state More...

virtual void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _dt2, Eigen::VectorXd &_delta1_plus_delta2, Eigen::MatrixXd &_jacobian1, Eigen::MatrixXd &_jacobian2) const =0
	composes a delta-state on top of another delta-state, and computes the Jacobians More...

virtual void	statePlusDelta (const VectorComposite &_x, const Eigen::VectorXd &_delta, const double _dt, VectorComposite &_x_plus_delta) const =0
	composes a delta-state on top of a state More...

virtual Eigen::VectorXd	deltaZero () const =0
	Delta zero. More...

virtual Eigen::VectorXd	correctDelta (const Eigen::VectorXd &delta_preint, const Eigen::VectorXd &delta_step) const =0
	correct the preintegrated delta This produces a delta correction according to the appropriate manifold. More...

void	mergeCaptures (CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target)
	merge two consecutive capture motions into the second one Merge two consecutive capture motions into the second one. The first capture motion is not removed. If the second capture has feature and factor emplaced, they are replaced by a new ones. More...

virtual Eigen::VectorXd	getCalibration (const CaptureBaseConstPtr _capture=nullptr) const =0

bool	hasCalibration () const

CaptureMotionConstPtr	getOrigin () const

CaptureMotionConstPtr	getLast () const

CaptureMotionConstPtr	getIncoming () const

CaptureMotionPtr	getOrigin ()

CaptureMotionPtr	getLast ()

CaptureMotionPtr	getIncoming ()

Eigen::MatrixXd	getUnmeasuredPerturbationCov () const

double	getMaxTimeSpan () const

double	getMaxBuffLength () const

double	getMaxDistTraveled () const

double	getMaxAngleTurned () const

void	setMaxTimeSpan (const double &_max_time_span)

void	setMaxBuffLength (const double &_max_buff_length)

void	setMaxDistTraveled (const double &_max_dist_traveled)

void	setMaxAngleTurned (const double &_max_angle_turned)

void	printHeader (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream, std::string _tabs="") const override

Public Member Functions inherited from wolf::ProcessorBase
void	startCaptureProfiling ()

void	stopCaptureProfiling ()

void	startKFProfiling ()

void	stopKFProfiling ()

virtual void	printProfiling (std::ostream &stream=std::cout) const

	ProcessorBase (const std::string &_type, const TypeComposite &_state_types, const YAML::Node &_params)
	constructor More...

	~ProcessorBase () override

virtual void	configure (SensorBasePtr _sensor)=0

virtual void	remove ()

bool	hasChildren () const override

const TypeComposite &	getStateTypes () const

StateKeys	getStateKeys () const

unsigned int	id () const

void	keyFrameCallback (FrameBasePtr _keyframe)
	notify a new keyframe made by another processor More...

void	captureCallback (CaptureBasePtr _capture)
	notify a new capture More...

SensorBaseConstPtr	getSensor () const

SensorBasePtr	getSensor ()

YAML::Node	getParams () const

bool	isMotionProvider () const

bool	applyLossFunction () const

bool	isVotingActive () const

double	getTimeTolerance () const

void	setApplyLossFunction (bool _apply_loss_function)

void	setVotingActive (bool _voting_active)

void	setTimeTolerance (double _time_tolerance)

void	setDebugVerboseLevel (const std::string &_debug_verbose_level)

std::string	getDebugVerboseLevel () const

bool	checkTimeTolerance (const TimeStamp &_ts1, const TimeStamp &_ts2) const

bool	checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp &_ts) const

bool	checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp &_ts) const

bool	checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const

virtual void	link (SensorBasePtr) final

virtual void	printHeader (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream, std::string _tabs="") const

void	print (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream=std::cout, std::string _tabs="") const

virtual CheckLog	localCheck (bool _verbose, std::ostream &_stream, std::string _tabs="") const

bool	check (CheckLog &_log, bool _verbose, std::ostream &_stream, std::string _tabs="") const

Public Member Functions inherited from wolf::NodeBase
	NodeBase (const std::string &_category, const std::string &_type="Undefined", const std::string &_name="")

virtual	~NodeBase ()=default

unsigned int	nodeId () const

std::string	getCategory () const

std::string	getType () const

std::string	getName () const

bool	isRemoving () const

virtual bool	hasChildren () const =0

void	setType (const std::string &_type)

void	setName (const std::string &_name)

ProblemConstPtr	getProblem () const

ProblemPtr	getProblem ()

Public Member Functions inherited from wolf::MotionProvider
	MotionProvider (const TypeComposite &_state_types, const YAML::Node &_params)
	Constructor for the MotionProvider class. More...

virtual	~MotionProvider ()
	Destructor for the MotionProvider class. More...

virtual TimeStamp	getTimeStamp () const =0
	Get the timestamp of the last motion. More...

virtual VectorComposite	getState (StateKeys _keys="") const =0
	Get the state at the last time. More...

virtual VectorComposite	getState (const TimeStamp &_ts, StateKeys _keys="") const =0
	Get the state at a specific time. More...

VectorComposite	getOdometry () const
	Get the odometry. More...

void	setOdometry (const VectorComposite &)
	Set the odometry. More...

bool	isStateProvider () const
	Check if the motion provider is configured as a state provider. More...

unsigned int	getOrder () const
	Get the order of the state provider. More...

void	setOrder (unsigned int)
	Set the order of the state provider. More...

Protected Member Functions
void	processCapture (CaptureBasePtr) override
	process an incoming capture More...

void	processKeyFrame (FrameBasePtr _keyframe_ptr) override
	process an incoming key-frame More...

bool	triggerInCapture (CaptureBasePtr) const override
	trigger in capture More...

bool	triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override
	trigger in key-frame More...

bool	storeKeyFrame (FrameBasePtr) override
	store key frame More...

bool	storeCapture (CaptureBasePtr) override
	store capture More...

bool	voteForKeyFrame () const override
	Vote for KeyFrame generation. More...

double	updateDt ()

void	integrateOneStep ()
	Make one step of motion integration. More...

void	reintegrateBuffer (CaptureMotionPtr _capture_ptr) const

void	splitBuffer (const wolf::CaptureMotionPtr &capture_source, const TimeStamp ts_split, const wolf::CaptureMotionPtr &capture_target) const

virtual void	preProcess ()

virtual void	bootstrap ()
	Bootstrap the processor with some initialization steps. More...

virtual void	postProcess ()

FrameBasePtr	computeProcessingStep ()

void	assertsCaptureMotion (CaptureMotionPtr _capture, std::string error_prefix) const

virtual CaptureMotionPtr	emplaceCapture (const FrameBasePtr &_frame_own, const SensorBasePtr &_sensor, const TimeStamp &_ts, const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib_preint, const CaptureBasePtr &_capture_origin_ptr)=0
	emplace a CaptureMotion More...

virtual void	emplaceFeaturesAndFactors (CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)=0
	emplace the features and factors corresponding to given capture and link them to the capture More...

virtual void	setCalibration (const CaptureBasePtr _capture, const Eigen::VectorXd &_calibration)=0

Motion	motionZero (const TimeStamp &_ts) const

Protected Member Functions inherited from wolf::ProcessorBase
virtual void	processCapture (CaptureBasePtr)=0
	process an incoming capture More...

virtual void	processKeyFrame (FrameBasePtr _keyframe_ptr)=0
	process an incoming key-frame More...

virtual bool	triggerInCapture (CaptureBasePtr) const =0
	trigger in capture More...

virtual bool	triggerInKeyFrame (FrameBasePtr) const =0
	trigger in key-frame More...

virtual bool	storeKeyFrame (FrameBasePtr)=0
	store key frame More...

virtual bool	storeCapture (CaptureBasePtr)=0
	store capture More...

virtual bool	voteForKeyFrame () const =0
	Vote for KeyFrame generation. More...

virtual bool	permittedKeyFrame () const final

void	setProblem (ProblemPtr) override

void	addTypes (const TypeComposite &)
	add state types More...

virtual void	setProblem (ProblemPtr _prob_ptr)

Protected Member Functions inherited from wolf::MotionProvider
void	addTypes (const TypeComposite &_state_types)
	Add types to the motion provider. More...

Protected Attributes
ProcessingStep	processing_step_
	State machine controlling the processing step. More...

bool	bootstrapping_
	processor is bootstrapping More...

SizeEigen	x_size_
	The size of the state vector. More...

SizeEigen	data_size_
	the size of the incoming data More...

SizeEigen	delta_size_
	the size of the deltas More...

SizeEigen	delta_cov_size_
	the size of the delta covariances matrix More...

SizeEigen	calib_size_
	the size of the calibration parameters (TBD in derived classes) More...

CaptureMotionPtr	origin_ptr_

CaptureMotionPtr	last_ptr_

CaptureMotionPtr	incoming_ptr_

double	dt_
	Time step. More...

Eigen::VectorXd	x_
	current state More...

Eigen::VectorXd	delta_
	current delta More...

Eigen::MatrixXd	delta_cov_
	current delta covariance More...

Eigen::VectorXd	delta_integrated_
	integrated delta More...

Eigen::MatrixXd	delta_integrated_cov_
	integrated delta covariance More...

Eigen::VectorXd	calib_preint_
	calibration vector used during pre-integration More...

Eigen::MatrixXd	jacobian_delta_preint_
	jacobian of delta composition w.r.t previous delta integrated More...

Eigen::MatrixXd	jacobian_delta_
	jacobian of delta composition w.r.t current delta More...

Eigen::MatrixXd	jacobian_calib_
	jacobian of delta preintegration wrt calibration params More...

Eigen::MatrixXd	jacobian_delta_calib_
	jacobian of delta wrt calib params More...

Eigen::MatrixXd	unmeasured_perturbation_cov_
	Covariance of unmeasured DoF to avoid singularity. More...

Protected Attributes inherited from wolf::ProcessorBase
unsigned int	processor_id_

YAML::Node	params_

BufferFrame	buffer_frame_

BufferCapture	buffer_capture_

std::string	debug_verbose_level_

Protected Attributes inherited from wolf::NodeBase
ProblemWPtr	problem_ptr_

unsigned int	node_id_
	Node id. It is unique over the whole Wolf Tree. More...

std::string	node_category_
	Text label identifying the category of node ("SENSOR", "FEATURE", etc) More...

std::string	node_type_
	Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) More...

std::string	node_name_

bool	is_removing_
	A flag for safely removing nodes from the Wolf tree. See remove(). More...

Protected Attributes inherited from wolf::MotionProvider
bool	is_state_provider_
	Whether this motion provider is configured as a state provider. More...

unsigned int	state_provider_order_
	If a state provider, the order to be used by the problem. More...

Detailed Description

class for Motion processors

This processor integrates motion data into vehicle states.

The motion data is provided by the sensor owning this processor.

This data is, in the general case, in the reference frame of the sensor:

Beware of the frame transformations Map to Robot, and Robot to Sensor, so that your produced motion Factors are correctly expressed.
- The robot state R is expressed in a global or 'Map' reference frame, named M.
- The sensor frame S is expressed in the robot frame R.
- The motion data data_ is expressed in the sensor frame S.
You can use three basic methods for this:
- The trivial: make the sensor frame and the robot frame the same frame, that is, S = Id.
- Transform incoming data from sensor frame to robot frame, and then integrate motion in robot frame.
- Integrate motion directly in sensor frame, and transform to robot frame at the time of:
  - Publishing the robot state (see getCurrentState() and similar functions)
  - Creating Keyframes and Factors (see emplaceFactor() ).

Should you need extra functionality for your derived types, you can overload these two methods,

preProcess() { }
postProcess() { }

which are called at the beginning and at the end of process(). See the doc of these functions for more info.

Member Enumeration Documentation

◆ ProcessingStep

enum wolf::ProcessorMotion::ProcessingStep

Enumerator
FIRST_TIME_WITHOUT_KF
FIRST_TIME_WITH_KF_BEFORE_INCOMING
FIRST_TIME_WITH_KF_ON_INCOMING
FIRST_TIME_WITH_KF_AFTER_INCOMING
RUNNING_WITHOUT_KF
RUNNING_WITH_KF_BEFORE_ORIGIN
RUNNING_WITH_KF_ON_ORIGIN
RUNNING_WITH_KF_AFTER_ORIGIN

Constructor & Destructor Documentation

◆ ProcessorMotion()

wolf::ProcessorMotion::ProcessorMotion	(	const std::string &	_type,
		TypeComposite	_state_types,
		SizeEigen	_state_size,
		SizeEigen	_delta_size,
		SizeEigen	_delta_cov_size,
		SizeEigen	_data_size,
		SizeEigen	_calib_size,
		const YAML::Node &	_params
	)

◆ ~ProcessorMotion()

wolf::ProcessorMotion::~ProcessorMotion ( )

override

Member Function Documentation

◆ assertsCaptureMotion()

void wolf::ProcessorMotion::assertsCaptureMotion	(	CaptureMotionPtr	_capture,
		std::string	error_prefix
	)		const

protected

◆ bootstrap()

virtual void wolf::ProcessorMotion::bootstrap ( )

inlineprotectedvirtual

Bootstrap the processor with some initialization steps.

◆ computeCurrentDelta()

virtual void wolf::ProcessorMotion::computeCurrentDelta	(	const Eigen::VectorXd &	_data,
		const Eigen::MatrixXd &	_data_cov,
		const Eigen::VectorXd &	_calib,
		const double	_dt,
		Eigen::VectorXd &	_delta,
		Eigen::MatrixXd &	_delta_cov,
		Eigen::MatrixXd &	_jacobian_calib
	)		const

pure virtual

convert raw CaptureMotion data to the delta-state format

This function accepts raw data and time step dt, and computes the value of the delta-state and its covariance. Note that these values are held by the members delta_ and delta_cov_.

Parameters

_data	measured motion data
_data_cov	covariance
_dt	time step
_delta	computed delta
_delta_cov	covariance
_calib	current state of the calibrated parameters
_jacobian_calib	Jacobian of the delta wrt calib

Rationale:

The delta-state format must be compatible for integration using the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta(). See the class documentation for some Eigen::VectorXd suggestions.

The data format is generally not the same as the delta format, because it is the format of the raw data provided by the Capture, which is unaware of the needs of this processor.

Additionally, sometimes the data format is in the form of a velocity, or a higher derivative, while the delta is in the form of an increment. In such cases, converting from data to delta-state needs integrating the data over the period dt.

Two trivial implementations would establish:

If _data is an increment:

 delta_     = _data;
 delta_cov_ = _data_cov

If _data is a velocity:
```
 delta_     = _data * _dt
 delta_cov_ = _data_cov * _dt.
```
However, other more complicated relations are possible. In general, we'll have a nonlinear function relating delta_ to _data and _dt, as follows:

delta_ = f ( _data, _dt)

The delta covariance is obtained by classical uncertainty propagation of the data covariance, that is, through the Jacobians of f(),

delta_cov_ = F_data * _data_cov * F_data.transpose

where F_data = d_f/d_data is the Jacobian of f().

Implemented in wolf::ProcessorDiffDrive, wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ computeProcessingStep()

FrameBasePtr wolf::ProcessorMotion::computeProcessingStep ( )

protected

◆ correctDelta()

virtual Eigen::VectorXd wolf::ProcessorMotion::correctDelta	(	const Eigen::VectorXd &	delta_preint,
		const Eigen::VectorXd &	delta_step
	)		const

pure virtual

correct the preintegrated delta This produces a delta correction according to the appropriate manifold.

Parameters

delta_preint	: the preintegrated delta to correct
delta_step	: an increment in the tangent space of the manifold

We want to do

delta_corr = delta_preint (+) d_step

where the operator (+) is the right-plus operator on the delta's manifold.

Note: usually in motion pre-integration we have

delta_step = Jac_delta_calib * (calib - calib_pre)

Implemented in wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ deltaPlusDelta() [1/2]

virtual void wolf::ProcessorMotion::deltaPlusDelta	(	const Eigen::VectorXd &	_delta1,
		const Eigen::VectorXd &	_delta2,
		const double	_dt2,
		Eigen::VectorXd &	_delta1_plus_delta2
	)		const

pure virtual

composes a delta-state on top of another delta-state

Parameters

_delta1	the first delta-state
_delta2	the second delta-state
_dt2	the second delta-state's time delta
_delta1_plus_delta2	the delta2 composed on top of delta1. It has the format of delta-state.

This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.

Implemented in wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ deltaPlusDelta() [2/2]

virtual void wolf::ProcessorMotion::deltaPlusDelta	(	const Eigen::VectorXd &	_delta1,
		const Eigen::VectorXd &	_delta2,
		const double	_dt2,
		Eigen::VectorXd &	_delta1_plus_delta2,
		Eigen::MatrixXd &	_jacobian1,
		Eigen::MatrixXd &	_jacobian2
	)		const

pure virtual

composes a delta-state on top of another delta-state, and computes the Jacobians

Parameters

_delta1	the first delta-state
_delta2	the second delta-state
_dt2	the second delta-state's time delta
_delta1_plus_delta2	the delta2 composed on top of delta1. It has the format of delta-state.
_jacobian1	the jacobian of the composition w.r.t. _delta1.
_jacobian2	the jacobian of the composition w.r.t. _delta2.

This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.

Implemented in wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ deltaZero()

virtual Eigen::VectorXd wolf::ProcessorMotion::deltaZero ( ) const

pure virtual

Delta zero.

Returns: a delta state equivalent to the null motion.

Examples (see documentation of the the class for info on Eigen::VectorXd):

2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!

Implemented in wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ emplaceCapture()

virtual CaptureMotionPtr wolf::ProcessorMotion::emplaceCapture	(	const FrameBasePtr &	_frame_own,
		const SensorBasePtr &	_sensor,
		const TimeStamp &	_ts,
		const Eigen::VectorXd &	_data,
		const Eigen::MatrixXd &	_data_cov,
		const Eigen::VectorXd &	_calib,
		const Eigen::VectorXd &	_calib_preint,
		const CaptureBasePtr &	_capture_origin_ptr
	)

protectedpure virtual

emplace a CaptureMotion

Parameters

_frame_own	frame owning the Capture to create
_sensor	Sensor that produced the Capture
_ts	time stamp
_data	a vector of motion data
_data_cov	covariances matrix of the motion data data
_calib	calibration vector
_calib_preint	calibration vector used during pre-integration
_capture_origin_ptr	capture acting as the origin of motions for the MorionBuffer of the created MotionCapture

Implemented in wolf::ProcessorDiffDrive, wolf::ProcessorOdom3d, and wolf::ProcessorOdom2d.

◆ emplaceFeaturesAndFactors()

virtual void wolf::ProcessorMotion::emplaceFeaturesAndFactors	(	CaptureBasePtr	_capture_origin,
		CaptureMotionPtr	_capture_own
	)

protectedpure virtual

emplace the features and factors corresponding to given capture and link them to the capture

Parameters

_capture_own	the parent capture
_capture_origin	the capture constrained by this motion factor

Typical factors to add for a ProcessorMotionDerived can be:

A preintegrated motion factor – this is the main factor
A calibration drift factor – only for dynamic sensor calibration parameters
An instantaneous factor for observing non-typical states of the Frame – useful for complex dynamic robot models

Implemented in wolf::ProcessorDiffDrive, wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ findCaptureContainingTimeStamp() [1/2]

CaptureMotionPtr wolf::ProcessorMotion::findCaptureContainingTimeStamp ( const TimeStamp & _ts )

◆ findCaptureContainingTimeStamp() [2/2]

CaptureMotionConstPtr wolf::ProcessorMotion::findCaptureContainingTimeStamp ( const TimeStamp & _ts ) const

Finds the capture that contains the closest previous motion of _ts.

Returns: a pointer to the capture (if it exists) or a nullptr (otherwise)

◆ getBuffer() [1/2]

MotionBuffer & wolf::ProcessorMotion::getBuffer ( )

inline

◆ getBuffer() [2/2]

const MotionBuffer & wolf::ProcessorMotion::getBuffer ( ) const

inline

◆ getCalibration()

virtual Eigen::VectorXd wolf::ProcessorMotion::getCalibration ( const CaptureBaseConstPtr _capture = nullptr ) const

pure virtual

Implemented in wolf::ProcessorOdom2d, wolf::ProcessorOdom3d, and wolf::ProcessorDiffDrive.

◆ getCurrentDeltaPreintCov()

const Eigen::MatrixXd wolf::ProcessorMotion::getCurrentDeltaPreintCov ( ) const

inline

Gets the delta preintegrated covariance from all integrations so far.

Returns: the delta preintegrated covariance matrix

◆ getMotion() [1/2]

Motion wolf::ProcessorMotion::getMotion ( ) const

inline

Provide the motion integrated so far.

Returns: the integrated motion

◆ getMotion() [2/2]

Motion wolf::ProcessorMotion::getMotion ( const TimeStamp & _ts ) const

inline

Provide the motion integrated until a given timestamp.

Returns: the integrated motion

◆ getOrigin() [1/2]

CaptureMotionPtr wolf::ProcessorMotion::getOrigin ( )

inline

◆ getOrigin() [2/2]

CaptureMotionConstPtr wolf::ProcessorMotion::getOrigin ( ) const

inline

◆ getState() [1/2]

VectorComposite wolf::ProcessorMotion::getState	(	const TimeStamp &	_ts,
		StateKeys	_keys = `""`
	)		const

overridevirtual

Get the state at a specific time.

Parameters

_ts	The timestamp of the state to retrieve
_keys	The keys of the states to retrieve if empty, all keys are retrieved

Returns: The state at the specified time

Implements wolf::MotionProvider.

◆ getState() [2/2]

VectorComposite wolf::ProcessorMotion::getState ( StateKeys _keys = "" ) const

overridevirtual

Get the state at the last time.

Parameters

_keys The keys of the states to retrieve if empty, all keys are retrieved

Returns: The state at the current time

Implements wolf::MotionProvider.

◆ getTimeStamp()

TimeStamp wolf::ProcessorMotion::getTimeStamp ( ) const

overridevirtual

Get the timestamp of the last motion.

Returns: The timestamp of the lastmotion

Implements wolf::MotionProvider.

◆ getUnmeasuredPerturbationCov()

Eigen::MatrixXd wolf::ProcessorMotion::getUnmeasuredPerturbationCov ( ) const

inline

◆ hasCalibration()

bool wolf::ProcessorMotion::hasCalibration ( ) const

inline

◆ integrateOneStep()

void wolf::ProcessorMotion::integrateOneStep ( )

protected

Make one step of motion integration.

Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_

◆ mergeCaptures()

void wolf::ProcessorMotion::mergeCaptures	(	CaptureMotionPtr	cap_prev,
		CaptureMotionPtr	cap_target
	)

merge two consecutive capture motions into the second one Merge two consecutive capture motions into the second one. The first capture motion is not removed. If the second capture has feature and factor emplaced, they are replaced by a new ones.

Parameters

cap_prev	: the first capture motion to be merged (input)
cap_target	: the second capture motion (modified)

◆ motionZero()

Motion wolf::ProcessorMotion::motionZero ( const TimeStamp & _ts ) const

inlineprotected

◆ postProcess()

virtual void wolf::ProcessorMotion::postProcess ( )

inlineprotectedvirtual

Post-process

This is called by process() after finishing the processing algorithm.

Overload this function to post-process stuff on derived classes.

Typical uses of postPrecess() are:

resetting and/or clearing variables and/or algorithms at the end of processing
drawing / printing / logging the results of the processing

◆ preProcess()

virtual void wolf::ProcessorMotion::preProcess ( )

inlineprotectedvirtual

Pre-process incoming Capture

This is called by process() just after assigning incoming_ptr_ to a valid Capture.

Overload this function to prepare stuff on derived classes.

Typical uses of prePrecess() are:

casting base types to derived types
initializing counters, flags, or any derived variables
initializing algorithms needed for processing the derived data

◆ printHeader()

void wolf::ProcessorMotion::printHeader	(	int	depth,
		bool	factored_by,
		bool	metric,
		bool	state_blocks,
		std::ostream &	stream,
		std::string	_tabs = `""`
	)		const

overridevirtual

Reimplemented from wolf::ProcessorBase.

◆ processCapture()

void wolf::ProcessorMotion::processCapture ( CaptureBasePtr _incoming_ptr )

overrideprotectedvirtual

process an incoming capture

Each derived processor should implement this function. It will be called if:

A new capture arrived and triggerInCapture() returned true.

Implements wolf::ProcessorBase.

◆ processKeyFrame()

void wolf::ProcessorMotion::processKeyFrame ( FrameBasePtr _keyframe_ptr )

inlineoverrideprotectedvirtual

process an incoming key-frame

The ProcessorMotion only processes incoming captures (it is not called).

Implements wolf::ProcessorBase.

◆ reintegrateBuffer()

void wolf::ProcessorMotion::reintegrateBuffer ( CaptureMotionPtr _capture_ptr ) const

protected

◆ resetDerived()

void wolf::ProcessorMotion::resetDerived ( )

inlinevirtual

◆ setCalibration()

virtual void wolf::ProcessorMotion::setCalibration	(	const CaptureBasePtr	_capture,
		const Eigen::VectorXd &	_calibration
	)

protectedpure virtual

Implemented in wolf::ProcessorDiffDrive, wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ setMaxAngleTurned()

void wolf::ProcessorMotion::setMaxAngleTurned ( const double & _max_angle_turned )

inline

◆ setMaxBuffLength()

void wolf::ProcessorMotion::setMaxBuffLength ( const double & _max_buff_length )

inline

◆ setMaxDistTraveled()

void wolf::ProcessorMotion::setMaxDistTraveled ( const double & _max_dist_traveled )

inline

◆ setMaxTimeSpan()

void wolf::ProcessorMotion::setMaxTimeSpan ( const double & _max_time_span )

inline

◆ setOrigin() [1/2]

FrameBasePtr wolf::ProcessorMotion::setOrigin	(	const VectorComposite &	_x_origin,
		const TimeStamp &	_ts_origin
	)

Set the origin of all motion for this processor

Parameters

_x_origin	the state at the origin
_ts_origin	origin timestamp.

◆ setOrigin() [2/2]

void wolf::ProcessorMotion::setOrigin ( FrameBasePtr _origin_frame )

Set the origin of all motion for this processor

Parameters

_origin_frame the keyframe to be the origin

◆ splitBuffer()

void wolf::ProcessorMotion::splitBuffer	(	const wolf::CaptureMotionPtr &	capture_source,
		const TimeStamp	ts_split,
		const wolf::CaptureMotionPtr &	capture_target
	)		const

protected

we are doing this:

Before split:

       ts_split

KF | F or KF

--------------------—* origin source

After split:

KF KF F or KF

-------—* --------—* origin target source

◆ statePlusDelta()

virtual void wolf::ProcessorMotion::statePlusDelta	(	const VectorComposite &	_x,
		const Eigen::VectorXd &	_delta,
		const double	_dt,
		VectorComposite &	_x_plus_delta
	)		const

pure virtual

composes a delta-state on top of a state

Parameters

_x	the initial state
_delta	the delta-state
_x_plus_delta	the updated state. It has the same format as the initial state.
_dt	the time interval spanned by the Delta

This function implements the composition (+) so that _x2 = _x1 (+) _delta.

Implemented in wolf::ProcessorOdom2d, and wolf::ProcessorOdom3d.

◆ storeCapture()

bool wolf::ProcessorMotion::storeCapture ( CaptureBasePtr )

inlineoverrideprotectedvirtual

store capture

Returns true if the capture should be stored

Implements wolf::ProcessorBase.

◆ storeKeyFrame()

bool wolf::ProcessorMotion::storeKeyFrame ( FrameBasePtr )

inlineoverrideprotectedvirtual

store key frame

Returns true if the key frame should be stored

Implements wolf::ProcessorBase.

◆ triggerInCapture()

bool wolf::ProcessorMotion::triggerInCapture ( CaptureBasePtr ) const

inlineoverrideprotectedvirtual

trigger in capture

Returns true if processCapture() should be called after the provided capture arrived.

Implements wolf::ProcessorBase.

◆ triggerInKeyFrame()

bool wolf::ProcessorMotion::triggerInKeyFrame ( FrameBasePtr _keyframe_ptr ) const

inlineoverrideprotectedvirtual

trigger in key-frame

The ProcessorMotion only processes incoming captures, then it returns false.

Implements wolf::ProcessorBase.

◆ updateDt()

double wolf::ProcessorMotion::updateDt ( )

inlineprotected

◆ voteForKeyFrame()

bool wolf::ProcessorMotion::voteForKeyFrame ( ) const

inlineoverrideprotectedvirtual

Vote for KeyFrame generation.

If a KeyFrame criterion is validated, this function returns true, meaning that it wants to create a KeyFrame at the last Capture.

WARNING! This function only votes! It does not create KeyFrames!

CaptureMotionPtr wolf::ProcessorMotion::incoming_ptr_

protected

◆ jacobian_calib_

Eigen::MatrixXd wolf::ProcessorMotion::jacobian_calib_

mutableprotected

jacobian of delta preintegration wrt calibration params

◆ jacobian_delta_

Eigen::MatrixXd wolf::ProcessorMotion::jacobian_delta_

mutableprotected

jacobian of delta composition w.r.t current delta

◆ jacobian_delta_calib_

Eigen::MatrixXd wolf::ProcessorMotion::jacobian_delta_calib_

mutableprotected

jacobian of delta wrt calib params

◆ jacobian_delta_preint_

Eigen::MatrixXd wolf::ProcessorMotion::jacobian_delta_preint_

mutableprotected

jacobian of delta composition w.r.t previous delta integrated

◆ last_ptr_

CaptureMotionPtr wolf::ProcessorMotion::last_ptr_

protected

◆ origin_ptr_

CaptureMotionPtr wolf::ProcessorMotion::origin_ptr_

protected

◆ processing_step_

ProcessingStep wolf::ProcessorMotion::processing_step_

protected

State machine controlling the processing step.

◆ unmeasured_perturbation_cov_

Eigen::MatrixXd wolf::ProcessorMotion::unmeasured_perturbation_cov_

protected

Covariance of unmeasured DoF to avoid singularity.

◆ x_

Eigen::VectorXd wolf::ProcessorMotion::x_

mutableprotected

current state

◆ x_size_

SizeEigen wolf::ProcessorMotion::x_size_

protected

The size of the state vector.

The documentation for this class was generated from the following files:

include/core/processor/processor_motion.h
src/processor/processor_motion.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Enumeration Documentation

◆ ProcessingStep

Constructor & Destructor Documentation

◆ ProcessorMotion()

◆ ~ProcessorMotion()

Member Function Documentation

◆ assertsCaptureMotion()

◆ bootstrap()

◆ computeCurrentDelta()

◆ computeProcessingStep()

◆ correctDelta()

◆ deltaPlusDelta() [1/2]

◆ deltaPlusDelta() [2/2]

◆ deltaZero()

◆ emplaceCapture()

◆ emplaceFeaturesAndFactors()

◆ findCaptureContainingTimeStamp() [1/2]

◆ findCaptureContainingTimeStamp() [2/2]

◆ getBuffer() [1/2]

◆ getBuffer() [2/2]

◆ getCalibration()

◆ getCurrentDeltaPreintCov()

◆ getIncoming() [1/2]

◆ getIncoming() [2/2]

◆ getLast() [1/2]

◆ getLast() [2/2]

◆ getMaxAngleTurned()

◆ getMaxBuffLength()

◆ getMaxDistTraveled()

◆ getMaxTimeSpan()

◆ getMotion() [1/2]

◆ getMotion() [2/2]

◆ getOrigin() [1/2]

◆ getOrigin() [2/2]

◆ getState() [1/2]

◆ getState() [2/2]

◆ getTimeStamp()

◆ getUnmeasuredPerturbationCov()

◆ hasCalibration()

◆ integrateOneStep()

◆ mergeCaptures()

◆ motionZero()

◆ postProcess()

◆ preProcess()

◆ printHeader()

◆ processCapture()

◆ processKeyFrame()

◆ reintegrateBuffer()

◆ resetDerived()

◆ setCalibration()

◆ setMaxAngleTurned()

◆ setMaxBuffLength()

◆ setMaxDistTraveled()

◆ setMaxTimeSpan()

◆ setOrigin() [1/2]

◆ setOrigin() [2/2]

◆ splitBuffer()

◆ statePlusDelta()

◆ storeCapture()

◆ storeKeyFrame()

◆ triggerInCapture()

◆ triggerInKeyFrame()

◆ updateDt()

◆ voteForKeyFrame()

Member Data Documentation

◆ bootstrapping_

◆ calib_preint_

◆ calib_size_

◆ data_size_

◆ delta_

◆ delta_cov_

◆ delta_cov_size_

◆ delta_integrated_

◆ delta_integrated_cov_

◆ delta_size_